#include <string.h>
#include <math.h>

#include <L1_hal_types.h>

#include "mul32.h"
#include "pidFXP.h"
#include "deriveFXP.h"
#include "integrateFXP.h"

/** 
 * \details
 * PID controller with actuator limit. 
 *
 * \param[in] inputQ24 - reference value in Q24 format
 * \param[in] sensorQ24 - actual value in Q24 format
 * \param pid internal memory
 *
 * \return actuator signal in Q8 format
 *
 * \ingroup Control
 */
L1_INT32 pidRunFXP(L1_INT32 inputQ24, L1_INT32 sensorQ24, PIDFXP* pid)
{
    // error calculation
    L1_INT32 errorQ24 = inputQ24 - sensorQ24;

    // calculate Proportional term
    L1_INT32 vQ8 = mul32(errorQ24, pid->PQ16);

    // calculate Integrate term with windup compensation
    vQ8 += integrateFXP(mul32(errorQ24,pid->IQ16) + mul32(pid->windupQ24,pid->WQ16), &pid->int_z);

    // calculate Derivative term
    vQ8 += deriveFXP(mul32(-sensorQ24,pid->DQ16), &pid->der_z);

    // actuator model
    L1_INT32 vLimQ8 = vQ8 > pid->LQ8 ? pid->LQ8 : vQ8;
    vLimQ8 = vLimQ8 < -pid->LQ8 ? -pid->LQ8 : vLimQ8;

    // calculate integrator windup
    pid->windupQ24 = (vLimQ8 - vQ8) << 16;

    return vLimQ8;    
}

/** 
 * \param pid internal memory
 * \param[in] P0 proportional coefficient
 * \param[in] I0 integration coefficient
 * \param[in] D0 derivative coefficient
 * \param[in] W0 anti-windup coefficient
 * \param[in] L0 actuator limit
 *
 * \ingroup Control
 */
void pidInitFXP(PIDFXP* pid, L1_INT32 P0, L1_INT32 I0, L1_INT32 D0, L1_INT32 W0, L1_INT32 L0)
{
    memset(pid, 0, sizeof(PIDFXP));
    
    pid->PQ16 = P0;
    pid->IQ16 = I0;
    pid->DQ16 = D0;
    pid->WQ16 = W0;
    pid->LQ8  = L0;
}
